// ManualBot.cpp : Defines the entry point for the console application.
//

#define _CRT_SECURE_NO_DEPRECATE 1
#include "stdafx.h"
#include "GridAgent.h"
#include "Visualizer.h"
#include "FollowWayPointsState.h"
#include "FollowTrackState.h"
#include "Track.h"

#include <glut.h>
#include <conio.h>   //for getch(), needed in wait_for_key()
#include <windows.h> //for Sleep()

void sleep(int i) { Sleep(i*1000); }
long GetTime();
void process_args(int argc, char* argv[]);

int _tmain(int argc, char* argv[]) {

	process_args(argc, argv);
	cout << "Show Visualizer? (y/n): ";
	char ch = (char)getch();
	cout << ch << endl;
	
	bool use_visualizer = ch == 'y' || ch == 'Y';

	BZRC* remote = Remote::instance();
	World* world = World::instance();

	world->init();

	std::vector<GridAgent> agents;
	for(unsigned int i = 0; i < world->tanks().size(); i ++){
		agents.push_back(GridAgent(&world->tanks()[i]));
	}

	int step = 50;
	int initial_height = world->getSize() / 2 - 25;
	int cur_height = initial_height;
	bool rev = false;

	std::vector<Vector2> way_points;

	for (unsigned int i = 0; i < agents.size(); i++) {
		if(world->getTrack()->getNextTrack(&way_points, false))
			agents.at(i).setState(new FollowTrackState(world->getGraph(), agents.at(i).getTank(), way_points, false));
		rev = !rev;
	}
	/*rev = false;
	for (unsigned int i = agents.size()/2; i < agents.size(); i++) {
		if(world->getTrack()->getNextTrack(&way_points, true))
			agents.at(i).setState(new FollowTrackState(world->getGraph(), agents.at(i).getTank(), way_points, true));
		rev = !rev;
	}*/

	Visualizer visualizer;
	visualizer.init(argc, argv);
	if (use_visualizer)
		visualizer.show();

	vector<Command> commands;

	int currentOccgrid = 0;
	int currentOccgrid2 = 1;
	int currentOccgrid3 = 2;

	while (true) {
		commands.clear();
		long t1 = GetTime();

		// Get commands for each of the tanks
		for(unsigned int i = 0; i < agents.size(); i ++){
			commands.push_back(agents[i].GetCommand());
		}

		//update occgrid for current tank
		commands.push_back(Command(currentOccgrid, -1, -1, false, true));
		//commands.push_back(Command(currentOccgrid2, -1, -1, false, true));


		// Update our understanding of the world
		commands.push_back(world->update());

		// Send all the commands
		remote->do_commands(commands);
		
		world->occupancy_probability_grid()->update(&world->occgridsP()->at(currentOccgrid));

		world->getGraph()->updateFromProbabilityGrid(&world->occgridsP()->at(currentOccgrid));

		//world->occupancy_probability_grid()->update(&world->occgridsP()->at(currentOccgrid2));

		//world->getGraph()->updateFromProbabilityGrid(&world->occgridsP()->at(currentOccgrid2));

		//world->occupancy_probability_grid()->update(&world->occgridsP()->at(currentOccgrid3));

		//world->getGraph()->updateFromProbabilityGrid(&world->occgridsP()->at(currentOccgrid3));

		currentOccgrid ++;
		//currentOccgrid2 ++;
		//currentOccgrid3 ++;
		if(currentOccgrid >= (int)world->tanks().size()) {
			currentOccgrid = 0;
		}
		//if(currentOccgrid2 >= (int)world->tanks().size()) {
		//	currentOccgrid2 = 0;
		//}
		//if(currentOccgrid3 >= (int)world->tanks().size()) {
		//	currentOccgrid3 = 0;
		//}

		long t2 = GetTime();
		cout << "Loop Time: " <<t2 - t1 << "ms" << endl;

		if(world->getTrack()->isDone()){
			world->printCorners();
			break;
		}
	}

	getchar();

	return 0;
}

void process_args(int argc, char* argv[]) {
	string server;
	int port;

	if(argc < 2) {
		cout << "server: ";
		cin >> server;
	}
	else {
		server = argv[1];
	}
    if(argc < 3) {
		cout << "port: ";
		cin >> port;
	}
	else {
        port = atoi(argv[2]);
    }

	Remote::init(server, port, false);

	if (!Remote::instance()->GetStatus()) {
		cout << "Unable to contact server" << endl;
		exit(1);
	}
}

long GetTime() {
	SYSTEMTIME tm;
	GetSystemTime(&tm);
	return (long)tm.wMilliseconds + 
		   (long)tm.wSecond * 1000 + 
		   (long)tm.wMinute * 60000 + 
		   (long)tm.wHour * 3600000;// + 
		   //(long)tm.wDay * 86400000;
}